#include <vector>

typedef std::size_t size_t;

class HybridTaus
	{
protected:
	unsigned _z1, _z2, _z3, _z4;

public:
	HybridTaus(unsigned z1, unsigned z2, unsigned z3, unsigned z4)
		: _z1(z1), _z2(z2), _z3(z3), _z4(z4)
		{}

	// S1, S2, S3, and M are all constants, and z is part of the generator state.  
	static inline unsigned tausStep(unsigned &z, int S1, int S2, int S3, unsigned M)  
		{  
	//	const unsigned b=(((z << S1) ^ z) >> S2);  
		return z = (((z & M) << S3) ^ (((z << S1) ^ z) >> S2));  
		}

	static inline unsigned lcgStep(unsigned & z, int A, unsigned C)
		{
		z = A * z + C;
	  	}

	inline unsigned operator()()  
		{  
		// combined period is lcm(p1,p2,p3,p4)~ 2^121  
		return   
			tausStep(_z1, 13, 19, 12, 4294967294ul) ^  // p1=2^31-1  
			tausStep(_z2, 2, 25, 4, 4294967288ul) ^    // p2=2^30-1  
			tausStep(_z3, 3, 11, 17, 4294967280ul) ^   // p3=2^28-1  
			lcgStep(_z4, 1664525, 1013904223ul)        // p4=2^32  
			;  
		}  
	};

template<class CONT, class EL>
inline void remove(CONT & c, const EL & e)
	{
	for (typename CONT::iterator i = c.begin(); i!= c.end(); i++)
		if (*i == e)
			{
			*i = c.back();
			c.pop_back();
			return;
			}
	}

HybridTaus rng(5093, 384938, 93010, 34384);

struct Individual
	{
	int x, y;
	int count;
	};

class Patch
	{
	std::vector<Individual> _population;

	typedef std::vector<std::vector<std::vector<size_t> > > cache_type;
	cache_type _cache;
	size_t _cell_size;

	size_t _max_x;
	size_t _max_y;

	int _visibility;
	int _max_sq_dist;

	inline size_t cache_pos(size_t x) const
		{
		return x / _cell_size;
		}
public:
	void init(size_t pop_size, size_t max_x, size_t max_y, int visibility, 
		size_t cache_cell_size)
		{
		_population.resize(pop_size);

		_max_x = max_x;
		_max_y = max_y;

		_cell_size = cache_cell_size;
		_cache.resize(_max_x/_cell_size);
		for (size_t x=0; x<_cache.size(); x++)
			_cache[x].resize(_max_y/_cell_size);
		
		_visibility = visibility;
		_max_sq_dist = _visibility * _visibility;

		for (size_t i=0; i<_population.size(); i++)
			{
			const int x = rng() % _max_x;
			const int y = rng() % _max_y;
			_population[i].x = x;
			_population[i].y = y;
			_population[i].count = 0;
			_cache[cache_pos(x)][cache_pos(y)].push_back(i);
			}
		}

	inline void step()
		{
		for (size_t i=0; i<_population.size(); i++)
			{
			Individual & ind = _population[i];

			const int cp_x = cache_pos(ind.x);
			const int cp_y = cache_pos(ind.y);

			const int new_x = (ind.x + rng()%5 - 2 + _max_x) % _max_x;
			const int new_y = (ind.y + rng()%5 - 2 + _max_y) % _max_y;

			const int cpn_x = cache_pos(new_x);
			const int cpn_y = cache_pos(new_y);

			if (cpn_x != cp_x || cpn_y != cp_y)
				{
				remove(_cache[cp_x][cp_y], i);
				_cache[cpn_x][cpn_y].push_back(i);
				}
			
			ind.x = new_x;
			ind.y = new_y;
			}

		const size_t cache_m_x = _cache.size();
		const size_t cache_m_y = _cache[0].size();

		for (size_t i=0; i<_population.size(); i++)
			{
			Individual & ind = _population[i];

			const int min_cp_x = cache_pos(ind.x - _visibility);
			const int min_cp_y = cache_pos(ind.y - _visibility);
			const int max_cp_x = cache_pos(ind.x + _visibility);
			const int max_cp_y = cache_pos(ind.y + _visibility);

			for (int cpx=min_cp_x; cpx<=max_cp_x; cpx++)
				{
				// remember whether we are beyond the boundary
				const int offs_x = cpx<0 ? -_max_x : 
					(cpx>=cache_m_x ? _max_x : 0);
				// but we also need the actual grid index
				const int rx = (cpx+cache_m_x)%cache_m_x;

				for (int cpy=min_cp_y; cpy<=max_cp_y; cpy++)
					{
					const int offs_y = cpy<0 ? -_max_y : 
						(cpy>=cache_m_y ? _max_y : 0);
					const int ry = (cpy+cache_m_y)%cache_m_y;

					const std::vector<size_t> & local = _cache[rx][ry];
					for (std::vector<size_t>::const_iterator l=local.begin(); l!=local.end(); l++)
						{
						// measure distance to projected position (needed for areas around edges)
						const int dx = ind.x - (_population[*l].x + offs_x);
						const int dy = ind.y - (_population[*l].y + offs_y);
						const int sq_dist = dx*dx + dy*dy;
						if (sq_dist < _max_sq_dist)
							ind.count++;
						}
					}
				}
			}
		}
	};

class World
	{
	std::vector<Patch> _patches;

public:
	
	World(size_t n_patches = 256, size_t pop_size = 256, 
		size_t max_x = 1024, size_t max_y = 1024, 
		size_t visibility = 10, size_t cache_cell_size = 64)
		{
		_patches.resize(n_patches);
		for (int i=0; i<_patches.size(); i++)
			_patches[i].init(pop_size, max_x, max_y, visibility, cache_cell_size);
		}

	inline void step()
		{
		for (int i=0; i<_patches.size(); i++)
			_patches[i].step();
		}
	};


int main()
	{
	World world;

	for (int s=0; s<1000; s++)
		world.step();
	}
